/*
******************************************************************************************************* 
**  Copyright (C) 2019, 苏州检易生物科技有限公司 
**  All rights reserved. 
** 
**  FileName:       	motor_config.c
**  Description:	
**  Author:        	 
**  Version				v0.1
**  Date:           	2019-08-06
**	Function List： 
**  History:         
*********************************************************************************************************
*/


/*
*********************************************************************************************************
*                                              	调试开关
*********************************************************************************************************
*/


/*
*********************************************************************************************************
*                                             INCLUDE FILES
*********************************************************************************************************
*/
#include "motor.h"
#include "log_console.h"
#include "timer.h"
#include "system_cfg.h"
#include "can_callback.h"
#include "can_task.h"
/*
*********************************************************************************************************
*                                              	宏定义
*********************************************************************************************************
*/

#define FLASH_SIZE_MOTOR_CFG (sizeof(motor_config_t)*MOTOR_NUM)

/*
*********************************************************************************************************
*                                              	结构体定义
*********************************************************************************************************
*/

 
/*
*********************************************************************************************************
*                                              	函数原型声明
*********************************************************************************************************
*/
static void can_motor_tx_callback(uint16_t can_id,uint8_t dlc,uint8_t* p_data,can_bus_err_t err);
static void can_motor_rx_callback(uint16_t can_id,uint8_t dlc,uint8_t* p_data,can_bus_err_t err);

static void motor_poll_timer_callback(hw_timer_t* handle);
/*
*********************************************************************************************************
*                                              	全局变量定义
*********************************************************************************************************
*/
extern motor_para_t m_module_cfg;
/*
*********************************************************************************************************
*                                              	模块静态变量定义
*********************************************************************************************************
*/
motor_t	m_motor_list[MOTOR_NUM];
motor_config_t	m_motor_cfg[MOTOR_NUM] = 
{
	/*20241109前版本*/
//	{.enable = 1,.node_id = 0x31,.node_index = 1,.current = 6,.speed = 200,.acc = 3,.mode_home = 2},/*玻片翻转*/
//	{.enable = 1,.node_id = 0x31,.node_index = 2,.current = 6,.speed = 200,.acc = 3,.mode_home = 2},/*玻片摇匀*/
//	{.enable = 1,.node_id = 0x32,.node_index = 1,.current = 8,.speed = 200,.acc = 3,.mode_home = 2},/*夹爪X向电机*/
//	{.enable = 1,.node_id = 0x33,.node_index = 1,.current = 8,.speed = 200,.acc = 3,.mode_home = 2},/*取样X*/
//	{.enable = 1,.node_id = 0x33,.node_index = 2,.current = 8,.speed = 200,.acc = 3,.mode_home = 2},/*样本旋转*/
//	{.enable = 1,.node_id = 0x34,.node_index = 1,.current = 8,.speed = 200,.acc = 3,.mode_home = 2},/*取样Y短向*/
//	{.enable = 1,.node_id = 0x35,.node_index = 1,.current = 8,.speed = 400,.acc = 3,.mode_home = 2},/*取样垂直*/
//	{.enable = 1,.node_id = 0x36,.node_index = 1,.current = 8,.speed = 200,.acc = 3,.mode_home = 4},/*夹爪短Y*/
//	{.enable = 1,.node_id = 0x37,.node_index = 1,.current = 8,.speed = 400,.acc = 3,.mode_home = 2},/*夹爪垂直*/
	
	/*20241109后改版版本*/
	{.enable = 1,.node_id = 0x31,.node_index = 1,.current = 10,.speed = 100,.acc = 1,.mode_home = 2},/*0玻片推入  温育中*/
	{.enable = 1,.node_id = 0x32,.node_index = 1,.current = 13,.speed = 100,.acc = 1,.mode_home = 4},/*1长纵轴 X*/
	{.enable = 1,.node_id = 0x31,.node_index = 2,.current = 8,.speed = 100,.acc = 1,.mode_home = 4},/*2样本旋转 Y*/
	{.enable = 1,.node_id = 0x34,.node_index = 1,.current = 13,.speed = 100,.acc = 1,.mode_home = 4},/*3夹爪旋转 */
	{.enable = 1,.node_id = 0x35,.node_index = 1,.current = 8,.speed = 200,.acc = 1,.mode_home = 2},/*4移液垂直 Y*/
	{.enable = 1,.node_id = 0x36,.node_index = 1,.current = 13,.speed = 100,.acc = 1,.mode_home = 4},/*5横轴 Y*/
	{.enable = 1,.node_id = 0x37,.node_index = 1,.current = 8,.speed = 300,.acc = 1,.mode_home = 2},/*6夹爪垂直 Y*/
	
	
	{.enable = 1,.node_id = 0x38,.node_index = 1,.current = 10,.speed = 300,.acc = 1,.mode_home = 4},/*7温育高*/
	{.enable = 1,.node_id = 0x38,.node_index = 2,.current = 10,.speed = 300,.acc = 1,.mode_home = 2},/*8温育低*/
	{.enable = 1,.node_id = 0x39,.node_index = 1,.current = 8,.speed = 300,.acc = 1,.mode_home = 4},/*9染色垂直*/
	{.enable = 1,.node_id = 0x39,.node_index = 2,.current = 8,.speed = 300,.acc = 1,.mode_home = 2},/*10染色水平*/ //
	{.enable = 1,.node_id = 0x3A,.node_index = 1,.current = 6,.speed = 150,.acc = 1,.mode_home = 4},/*11柱塞泵*/
	
};
/*
*********************************************************************************************************
*                                              	函数定义
*********************************************************************************************************
*/
void motor_config_init(void)
{
	bsp_timer_callback_register(&TIM_1K,motor_poll_timer_callback);
	motor_config_t* p_motor_cfg = m_motor_cfg;
	if(p_motor_cfg!=NULL)
	{
		for(uint8_t i=0;i<MOTOR_NUM;i++)
		{
			can_pct_regist(CAN_BUS_MOTOR,p_motor_cfg[i].node_id,can_motor_tx_callback,can_motor_rx_callback);
			m_motor_list[i].motor_id = i;
			m_motor_list[i].status = MOTOR_STATE_UNINIT;
			m_motor_list[i].tar_pos = 0;
			m_motor_list[i].sdo_cnt = 0;
			m_motor_list[i].mode	  = M_MODE_POS;
			m_motor_list[i].p_motor_config = &p_motor_cfg[i];
		}
	}
	return;
}
/*
	1ms硬件中断,电机轮询间隔 轮询总体状态寄存器BOARD_REG_STATUS 总体错误寄存器 BOARD_REG_ERR
	单个轮询运动电机,控制轮询间隔 防止总线拥堵 内存溢出
*/
static void motor_poll_timer_callback(hw_timer_t* handle)
{
	if(can_get_motor_list_size() >	CAN_SEND_LIST_MAX/2)
		return;
	rt_base_t it = rt_hw_interrupt_disable();
	motor_t* p_motor = NULL;
	if(handle->cnt % CAN_TPD_POLL_TIME_TICK == 0)
	{
	}
	if(handle->cnt % CAN_MOTOR_POLL_TIME_TICK == 0)
	{
		for(uint8_t idx = 0;idx < MOTOR_NUM;idx++)
		{
			p_motor = &m_motor_list[idx];
			if( p_motor->sdo_cnt == 0 && ( p_motor->status == MOTOR_STATE_STEP
				|| p_motor->status == MOTOR_STATE_SURFACE 
				|| p_motor->status == MOTOR_STATE_HALT
#if MOTOR_HOME_TYPE_OLD
#else
				|| p_motor->status == MOTOR_STATE_HOMING
#endif
			) )
			{
				can_mem_node_t* p_poll = can_mem_malloc();p_motor->sdo_cnt++;
				motor_read(p_motor->p_motor_config,M_REG_STATUS,&p_poll->can_msg);
				app_msg_send(CAN_MSG_PUSH,(uint32_t)p_poll,&g_can_task.mailbox);
			}
#if MOTOR_HOME_TYPE_OLD
			else if(p_motor->sdo_cnt == 0 && p_motor->status == MOTOR_STATE_HOMING)
			{
				can_mem_node_t* p_poll = can_mem_malloc();p_motor->sdo_cnt++;
				motor_read(p_motor->p_motor_config,M_REG_HOMING,&p_poll->can_msg);
				app_msg_send(CAN_MSG_PUSH,(uint32_t)p_poll,&g_can_task.mailbox);
			}
#endif
			/*液面探测初始化轮询*/
			if(p_motor->sdo_cnt == 0 && p_motor->surface_status == P_INITING)
			{
				can_mem_node_t* p_poll = can_mem_malloc();p_motor->sdo_cnt++;
				motor_read(p_motor->p_motor_config,M_REG_SURFACE_STATUS,&p_poll->can_msg);
				app_msg_send(CAN_MSG_PUSH,(uint32_t)p_poll,&g_can_task.mailbox);
			}
			
		}
	}
	rt_hw_interrupt_enable(it);
}
static void can_motor_tx_callback(uint16_t can_id,uint8_t dlc,uint8_t* p_data,can_bus_err_t err)
{
	
}
/*电机驱动板的接收回调函数*/
static void can_motor_rx_callback(uint16_t can_id,uint8_t dlc,uint8_t* p_data,can_bus_err_t err)
{
	uint16_t motor_id = 0;
	rt_base_t it = rt_hw_interrupt_disable();
	moto_data_t* p_moto_data = (moto_data_t*)p_data;
	if(CAN_ID_GET_FN(can_id) == FN_SDO_R)
	{
		motor_t* p_motor = get_motor(CAN_ID_GET_NODE(can_id),SDO_ADDR_TO_MOTOR_INDEX(p_moto_data->sdo_addr));
		if(p_motor == NULL)/*该通道不是电机*/
		{
			rt_hw_interrupt_enable(it);
			return;
		}
		/*是SDO命令且属于相应电机而不是总驱动板 则对相应电机sdo_cnt--*/
		if(p_moto_data->sdo_addr > MOTOR_ADDR_BASE && p_motor != NULL && p_motor->sdo_cnt)
		{
			p_motor->sdo_cnt--;
		}
		if(p_moto_data->sdo_cs == SDO_CS_ERR)
		{
			if(dlc == 2)
				Critical_Log("Can sdo err,0x%02X;\r\n",p_data[1]);
			else if(dlc == 4)
				Critical_Log("Can id 0x%03X sdo err,0x%02X_0x%02X_0x%02X;\r\n",can_id,p_data[1],p_data[2],p_data[3]);
		}
		else if(p_moto_data->sdo_cs == SDO_CS_WRITE_ACK)
		{
		}
		/*sdo读取寄存器的回复*/
		else if(p_moto_data->sdo_cs == SDO_CS_READ_ACK)
		{
			/*读取单个电机*/
			if(p_moto_data->sdo_addr > MOTOR_ADDR_BASE && p_motor != NULL)
			{
				switch(p_moto_data->regaddr)
				{
					case M_REG_ENC_POS:
						p_motor->enc_pos = p_moto_data->data;
						break;
					case M_REG_STATUS:
						if(p_motor->sdo_cnt != 0)
							break;
						else if(p_motor->status == MOTOR_STATE_HOMING)
						{
							if(p_moto_data->data & MSTATUS_HOME_TRIG)
							{
								tcp_err_push(MECH_ERR_PROBE_COL,"M%02d probe collision!",p_motor->motor_id);
								p_motor->status = MOTOR_STATE_ERR;
							}
							else if((p_moto_data->data & M_STATUS_STOP) 
//								&& (p_moto_data->data & M_STATUS_IN_POSITION)
								&& (p_moto_data->data & MSTATUS_HOME_CPLT))
							{
								Trace_Log("M%02d HOMED;T:%d;\r\n",p_motor->motor_id,(uint32_t)LocalTime);
								p_motor->status = MOTOR_STATE_IDLE;
							}
						}
						else if(p_motor->status == MOTOR_STATE_STEP)
						{
							if(p_moto_data->data & MSTATUS_HOME_TRIG)
							{
								tcp_err_push(MECH_ERR_PROBE_COL,"M%02d probe collision!",p_motor->motor_id);
								p_motor->status = MOTOR_STATE_ERR;
							}
							else if((p_moto_data->data & M_STATUS_STOP) && (p_moto_data->data & M_STATUS_IN_POSITION))
							{
								Trace_Log("M%02d stoped;T:%d;\r\n",p_motor->motor_id,(uint32_t)LocalTime);
								p_motor->status = MOTOR_STATE_IDLE;
								tcp_transfer_push(RPLY_JOG,&motor_id,2);
							}
						}
						else if(p_motor->status == MOTOR_STATE_SURFACE)
						{
							if(p_moto_data->data & MSTATUS_HOME_TRIG)
							{
								tcp_err_push(MECH_ERR_PROBE_COL,"M%02d probe collision!",p_motor->motor_id);
								p_motor->status = MOTOR_STATE_ERR;
							}
							else if((p_moto_data->data & M_STATUS_STOP) && (p_moto_data->data & M_STATUS_IN_POSITION))
							{
								Trace_Log("M%02d stoped;T:%d;\r\n",p_motor->motor_id,(uint32_t)LocalTime);
								p_motor->status = MOTOR_STATE_IDLE;
							}
						}
						else if(p_motor->status == MOTOR_STATE_SPEED)
						{
							if(p_moto_data->data & M_STATUS_STOP)
							{
								Trace_Log("M%02d stoped;T:%d;\r\n",p_motor->motor_id,(uint32_t)LocalTime);
								p_motor->status = MOTOR_STATE_IDLE;
							}
						}
						else if(p_motor->status == MOTOR_STATE_HALT)
						{
							if(p_moto_data->data & M_STATUS_STOP)
							{
								p_motor->status = MOTOR_STATE_IDLE;
							}
						}
#if MOTOR_HOME_TYPE_OLD
#else
						if(p_moto_data->data & M_STATUS_STALL)
						{
							/*检测到堵转 怎么处理TBD*/
							tcp_err_push(MECH_ERR_STALL,"M%02d detect stall!;\n",p_motor->motor_id,p_moto_data->data);
							p_motor->status = MOTOR_STATE_ERR;
						}
#endif
						if(p_moto_data->data & M_STATUS_ERR)
						{
							tcp_err_push(ELEC_ERR_MOTOR,"M%02d err,status 0x%08X;\n",p_motor->motor_id,p_moto_data->data);
							p_motor->status = MOTOR_STATE_ERR;
						}
						break;
					case M_REG_ERR:
						if(p_moto_data->data != 0)
						{
							p_motor->err_code = p_moto_data->data;
							if(p_motor->err_code == 0)
							{
								p_motor->err_code = 0xFFFFFFFF;
							}
							p_motor->status = MOTOR_STATE_ERR;
							Error_Log("Motor id %d err,node_id:0x%03X;index:%d,errcode:0x%08X;\r\n",p_motor->motor_id,
								CAN_ID_GET_NODE(can_id),SDO_ADDR_TO_MOTOR_INDEX(p_moto_data->sdo_addr),p_moto_data->data);
						}
						break;
					case M_REG_POS:/*液面探测时同步电机当前位置*/
//						if(p_motor->tar_pos == p_moto_data->data)
//						{
//							tcp_err_push(MECH_ERR_SP_SURFACE,"M%02d surface detect err;\n",p_motor->motor_id);
//							Critical_Log("Motor index %d pos %d equal to max pos %d,surface dection err!\r\n",p_motor->motor_id,p_motor->tar_pos,p_moto_data->data);
//						}
//						else
							Trace_Log("Motor update actual pos %d;\r\n",p_moto_data->data);
						p_motor->tar_pos = p_moto_data->data;
						break;
					case M_REG_HOMING:
						if(p_motor->status == MOTOR_STATE_HOMING && p_moto_data->data == 1)
						{
							p_motor->status = MOTOR_STATE_IDLE;
						}
						break;
					case M_REG_SURFACE_STATUS:
//						if(p_moto_data->data == P_DETECTED)
//						{
//							p_motor->surface_status = p_moto_data->data;
//							Trace_Log("M%02d surface detected;\r\n",p_motor->motor_id);
//						}
						if(p_motor->surface_status == P_INITING)
						{
							if(P_ERR == p_moto_data->data)
								tcp_err_push(ELEC_ERR_SP_SFINIT,"M%02d surface init err;\n",p_motor->motor_id);
						}
						p_motor->surface_status = p_moto_data->data;
						break;
					default:break;
				}
			}
			/*读取驱动板寄存器TBD*/
			else
			{
				switch(p_moto_data->sdo_addr)
				{
					case BOARD_REG_STATUS:break;
					case BOARD_REG_ERR:break;
					default:break;
				}
			}
		}
	}
	else if(CAN_ID_GET_FN(can_id) == FN_NMT_TIM)
	{
	}
	else if(CAN_ID_GET_FN(can_id) == FN_TPDO1)
	{
		
	}
	else if(CAN_ID_GET_FN(can_id) == FN_TPDO2)
	{
	}
	else if(CAN_ID_GET_FN(can_id) == FN_EMGC)
	{
	}
	rt_hw_interrupt_enable(it);
}
motor_t* get_motor(uint16_t node_id, uint8_t node_index)
{
	motor_t* p_motor;
	for(uint8_t i=0; i<MOTOR_NUM; i++)
	{
		p_motor = &m_motor_list[i];
		
		if((p_motor->p_motor_config->node_id == node_id) 
			&&(p_motor->p_motor_config->node_index == node_index)
		  && p_motor->p_motor_config->enable)
		{
			return p_motor;
		}
	}
	
	return NULL;
}
motor_t* id_get_motor(uint8_t motor_id)
{
	motor_t* p_motor;
	
	for(uint8_t i=0; i<MOTOR_NUM; i++)
	{
		p_motor = &m_motor_list[i];
		
		if(p_motor->motor_id == motor_id)
		{
			return p_motor;
		}
	}
	return NULL;	
}

uint32_t get_motor_current(uint8_t motor_id)
{
	ASSERT(motor_id < MOTOR_NUM);
	uint32_t current = 0;
	motor_t* p_motor = id_get_motor(motor_id);
#if MOTOR_CURRENT_STATIC
	return motor_current[p_motor->motor_id];
#endif
	current = p_motor->p_motor_config->current;
	if(current > 31)
	{
		Critical_Log("M%02d current illegal!\n",p_motor->motor_id);
		return 0;
	}
	return (((current/2)<<16)|current);
}
//motor_acc_t* get_motor_acc(uint8_t motor_id)
//{
//	ASSERT(motor_id < MOTOR_NUM);
//#if PRODUCT_NAME_IVD_DEBUG
//	return NULL;
//#endif
//	return &motor_acc[motor_id];
//}